// Copyright 2021 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "vehicle_constants_manager/vehicle_constants_manager.hpp"
#include <rclcpp/rclcpp.hpp>

#include <chrono>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

namespace autoware
{
namespace common
{
namespace vehicle_constants_manager
{

using float64_t = VehicleConstants::float64_t;


VehicleConstants::VehicleConstants(
  float64_t wheel_radius,
  float64_t wheel_width,
  float64_t wheel_base,
  float64_t wheel_tread,
  float64_t overhang_front,
  float64_t overhang_rear,
  float64_t overhang_left,
  float64_t overhang_right,
  float64_t vehicle_height,
  float64_t cg_to_rear,
  float64_t tire_cornering_stiffness_front,
  float64_t tire_cornering_stiffness_rear,
  float64_t mass_vehicle,
  float64_t inertia_yaw_kg_m_2)
: wheel_radius(wheel_radius),
  wheel_width(wheel_width),
  wheel_base(wheel_base),
  wheel_tread(wheel_tread),
  overhang_front(overhang_front),
  overhang_rear(overhang_rear),
  overhang_left(overhang_left),
  overhang_right(overhang_right),
  vehicle_height(vehicle_height),
  cg_to_rear(cg_to_rear),
  tire_cornering_stiffness_front(tire_cornering_stiffness_front),
  tire_cornering_stiffness_rear(tire_cornering_stiffness_rear),
  mass_vehicle(mass_vehicle),
  inertia_yaw_kg_m2(inertia_yaw_kg_m_2),
  cg_to_front(wheel_base - cg_to_rear),
  vehicle_length(overhang_front + wheel_base + overhang_rear),
  vehicle_width(overhang_left + wheel_tread + overhang_right),
  offset_longitudinal_min(-overhang_rear),
  offset_longitudinal_max(wheel_base + overhang_front),
  offset_lateral_min(-(wheel_tread / 2.0 + overhang_right)),
  offset_lateral_max(wheel_tread / 2.0 + overhang_left),
  offset_height_min(-wheel_radius),
  offset_height_max(vehicle_height - wheel_radius)
{
  // Sanity Checks
  // Center of gravity must be between front and rear axles
  if (wheel_base < cg_to_rear) {
    throw std::runtime_error("wheel_base must be larger than cg_to_rear");
  }

  // These values must be positive
  auto throw_if_negative = [](float64_t number, const std::string & name) {
      if (number < 0.0) {
        throw std::runtime_error(
                name + " = " + std::to_string(number) +
                " shouldn't be negative.");
      }
    };
  throw_if_negative(wheel_radius, "wheel_radius");
  throw_if_negative(wheel_width, "wheel_width");
  throw_if_negative(wheel_base, "wheel_base");
  throw_if_negative(wheel_tread, "wheel_tread");
  throw_if_negative(overhang_front, "overhang_front");
  throw_if_negative(overhang_rear, "overhang_rear");
  throw_if_negative(overhang_left, "overhang_left");
  throw_if_negative(overhang_right, "overhang_right");
  throw_if_negative(vehicle_height, "vehicle_height");
  throw_if_negative(cg_to_rear, "cg_to_rear");
  throw_if_negative(tire_cornering_stiffness_front, "tire_cornering_stiffness_front");
  throw_if_negative(tire_cornering_stiffness_rear, "tire_cornering_stiffness_rear");
  throw_if_negative(mass_vehicle, "mass_vehicle");
  throw_if_negative(inertia_yaw_kg_m_2, "inertia_yaw_kg_m_2");
}

std::string VehicleConstants::str_pretty() const
{
  return std::string{
    "wheel_radius: " + std::to_string(wheel_radius) + "\n"
    "wheel_width: " + std::to_string(wheel_width) + "\n"
    "wheel_base: " + std::to_string(wheel_base) + "\n"
    "wheel_tread: " + std::to_string(wheel_tread) + "\n"
    "overhang_front: " + std::to_string(overhang_front) + "\n"
    "overhang_rear: " + std::to_string(overhang_rear) + "\n"
    "overhang_left: " + std::to_string(overhang_left) + "\n"
    "overhang_right: " + std::to_string(overhang_right) + "\n"
    "vehicle_height: " + std::to_string(vehicle_height) + "\n"
    "cg_to_rear: " + std::to_string(cg_to_rear) + "\n"
    "tire_cornering_stiffness_front: " + std::to_string(tire_cornering_stiffness_front) + "\n"
    "tire_cornering_stiffness_rear: " + std::to_string(tire_cornering_stiffness_rear) + "\n"
    "mass_vehicle: " + std::to_string(mass_vehicle) + "\n"
    "inertia_yaw_kg_m2: " + std::to_string(inertia_yaw_kg_m2) + "\n"
    "cg_to_front: " + std::to_string(cg_to_front) + "\n"
    "vehicle_length: " + std::to_string(vehicle_length) + "\n"
    "vehicle_width: " + std::to_string(vehicle_width) + "\n"
    "offset_longitudinal_min: " + std::to_string(offset_longitudinal_min) + "\n"
    "offset_longitudinal_max: " + std::to_string(offset_longitudinal_max) + "\n"
    "offset_lateral_min: " + std::to_string(offset_lateral_min) + "\n"
    "offset_lateral_max: " + std::to_string(offset_lateral_max) + "\n"
    "offset_height_min: " + std::to_string(offset_height_min) + "\n"
    "offset_height_max: " + std::to_string(offset_height_max) + "\n"
  };
}

VehicleConstants try_get_vehicle_constants(
  const rclcpp::Node::SharedPtr & node_ptr,
  const std::chrono::nanoseconds & nanos_timeout)
{
  auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(
    node_ptr, "vehicle_constants_manager_node");
  using namespace std::chrono_literals;
  auto tp_start = node_ptr->now();
  // Wait until node is available
  while (!parameters_client->wait_for_service(1s)) {
    if (!rclcpp::ok()) {
      throw std::runtime_error("Interrupted while waiting for the vehicle_constants_manager_node.");
    }
    if (node_ptr->now() - tp_start > nanos_timeout) {
      throw std::runtime_error(
              "Timeout reached while attempting to reach vehicle_constants_manager_node");
    }
    RCLCPP_WARN(
      node_ptr->get_logger(), "vehicle_constants_manager_node is not available, waiting...");
  }

  // Wait until all parameters are published
  while (!parameters_client->has_parameter("published_all")) {
    if (!rclcpp::ok()) {
      throw std::runtime_error("Interrupted while waiting for parameters to be published.");
    }
    if (node_ptr->now() - tp_start > nanos_timeout) {
      throw std::runtime_error(
              "Timeout reached while waiting for all parameters to be published");
    }
    RCLCPP_WARN(node_ptr->get_logger(), "Not all parameters are published yet, waiting...");
    rclcpp::sleep_for(500ms);
  }

  const std::vector<std::string> names_params{
    "wheel_radius",
    "wheel_width",
    "wheel_base",
    "wheel_tread",
    "overhang_front",
    "overhang_rear",
    "overhang_left",
    "overhang_right",
    "vehicle_height",
    "cg_to_rear",
    "tire_cornering_stiffness_front",
    "tire_cornering_stiffness_rear",
    "mass_vehicle",
    "inertia_yaw_kg_m2"
  };

  std::vector<rclcpp::Parameter> params = parameters_client->get_parameters(names_params);

  if (params.size() != names_params.size()) {
    throw std::runtime_error("Not all parameters were retrieved.");
  }

  return VehicleConstants(
    params.at(0).as_double(),
    params.at(1).as_double(),
    params.at(2).as_double(),
    params.at(3).as_double(),
    params.at(4).as_double(),
    params.at(5).as_double(),
    params.at(6).as_double(),
    params.at(7).as_double(),
    params.at(8).as_double(),
    params.at(9).as_double(),
    params.at(10).as_double(),
    params.at(11).as_double(),
    params.at(12).as_double(),
    params.at(13).as_double()
  );
}
}  // namespace vehicle_constants_manager
}  // namespace common
}  // namespace autoware
